#include "board.h"

//循迹pid结构体
PID tracing_pid;

int new_error = 0;//循迹偏差数
int i;//循迹状态标志位（0：全部向左转; 1：全部向右转; 3：正常循迹状态）

//循迹pid初始化
void tracing_pid_init(void)
{
    pid_init(&tracing_pid, 70, 0, 80, 0, 4999, 0);
}

//循迹pid数据读取
PID* get_position_pid(void)
{
	return &tracing_pid;
}

//循迹pid的目标速度读取
int get_position_pid_target(void)
{
	return tracing_pid.target;
}

//循迹pid控制电机
void track_Set_PWM(int pwm_value)
{
	//基础速度+PID值
	int left_motor_speed = Car_Base_Speed - pwm_value;
	int right_motor_speed = Car_Base_Speed + pwm_value;
	set_all_motor(left_motor_speed,right_motor_speed);
}


/******************************************************************
 * 函 数 名 称：position_control
 * 函 数 说 明：循迹pid调用
 * 函 数 形 参：
 * 函 数 返 回:
 * 作       者：
 * 备       注：无
******************************************************************/
void position_control(void)
{
	new_error = tracing_Digital();
	if (i == 0)
	{
		if (new_error > 0)
			new_error = -new_error;
	}
	if (i == 1)
	{
		if (new_error < 0) 
			new_error = -new_error;
	}

	track_Set_PWM(pid_calc(&tracing_pid, 0, new_error));
}

